#include "vision_task.h"
#include "usb_task.h"
#include "SolveTrajectory.h"
#include "referee.h"
#include "data_exchange_center.h"
#include "user_lib.h"
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include "gimbal_task.h"

ReceiveDataVision_s *VisionData;
struct SolveTrajectoryParams stc;
Imu_t *IMU_v;
SolveAns ans;

// // 全局滤波器集合
// VisionDataFilters vision_filters;

static KalmanFilter pitch_filter;

const gimbal_motor_t *gimbal_vis;

int BallisticsSolve(void);
void InitVisionData(void);
void vision_debug(void);

void vision_task(void const *argument)
{
    InitVisionData();
    float last_time = HAL_GetTick();

    while (1)
    {

        get_robot_shoot_speed(&stc.current_v);
        if (stc.current_v < 10 && stc.vis_data.tracking)
        {
            stc.current_v = 100;
        }
        stc.current_pitch = -IMU_v->pitch;
        stc.current_yaw = IMU_v->yaw;

        memcpy(&stc.vis_data, &VisionData->data, sizeof(stc.vis_data));
        // 调试函数
        //  vision_debug();

        if (stc.vis_data.tracking)
        {
            stc.bias_time = HAL_GetTick() - VisionData->time_stamp;
        }
        else
        {
            stc.bias_time = 100;
        }

        if (VisionData->data.tracking)
        {
            autoSolveTrajectory(&stc, &ans.pitch_v, &ans.yaw_v, &ans.aim_x, &ans.aim_y, &ans.aim_z);
            float dt_vis = VISION_TIME / 1000.0f;
            float raw_pitch = ans.pitch_v;
            float filtered_pitch = Kalman_Update(&pitch_filter, raw_pitch, (raw_pitch - pitch_filter.angle) / dt_vis, dt_vis);

            ans.pitch_v = filtered_pitch;
            ans.tracking = stc.vis_data.tracking;
        }
        else
        {
            memset(&ans, 0, sizeof(SolveAns));
            ans.pitch_v = 0;
        }

        HAL_Delay(VISION_TIME);
    }
}

void InitVisionData()
{

    // srand((unsigned int)time(NULL));

    memset(&ans, 0, sizeof(SolveAns));

    gimbal_vis = get_pitch_motor_point();
    /*********************************部分变量********************************* */
    IMU_v = DataExchangeCenterSubscribe(IMU_NAME);
    VisionData = GetVisionData();
    // 定义固定参数
    stc.k = 0.038;               // 0.092                  //弹道系数
    stc.bullet_type = BULLET_17; // 自身机器人类型
    stc.vis_data.dz = 0.0;
    stc.bias_time = 100;
    stc.s_bias = 0.193190;
    stc.z_bias = 0.0; // 0.402726;

    // 变化
    stc.current_v = 18;    // 当前弹速
    stc.current_pitch = 0; // 当前pitch
    stc.current_yaw = 0;   // 当前pitch

    stc.vis_data.xw = 3.0;
    stc.vis_data.yw = 0;
    stc.vis_data.zw = 1.5;

    stc.vis_data.vxw = 0;
    stc.vis_data.vyw = 0;
    stc.vis_data.vzw = 0;

    stc.vis_data.v_yaw = 0;
    stc.vis_data.tar_yaw = 0.09131;

    stc.vis_data.r1 = 0.0;
    stc.vis_data.r2 = 0.0;

    Kalman_Init(&pitch_filter, 0.0001f, 0.0003f, 0.05f, 0.1f);
}

SolveAns *GetSolveAns(void)
{
    return &ans;
}

bool get_tracking_status(void)
{
    return ans.tracking;
}

void vision_debug()
{
    /*********************************部分变量********************************* */
    // 定义固定参数
    stc.k = 0.038;               // 0.092                  //弹道系数
    stc.bullet_type = BULLET_17; // 自身机器人类型
    // stc.vis_data.dz = 0.0;
    // stc.bias_time = 100;
    // stc.s_bias = 0.193190;
    stc.z_bias = 0; // 0.7;//0.402726;

    // 变化
    //  stc.current_v = 199999;             //当前弹速
    //  stc.current_pitch = 2;          //当前pitch
    //  stc.current_yaw = 2;            //当前pitch

    // ROS坐标
    //  stc.vis_data.xw = 3.0;
    //  stc.vis_data.yw = 0;
    //  stc.vis_data.zw = 1.5;

    // ROS速度
    //  stc.vis_data.vxw = 0;
    //  stc.vis_data.vyw = 0;
    //  stc.vis_data.vzw = 0;

    // 目标yaw
    //  stc.vis_data.v_yaw = 0;
    //  stc.vis_data.tar_yaw = 0.09131;

    // stc.vis_data.r1 = 1.0;
    // stc.vis_data.r2 = 1.0;
}

